#include <Arduino.h>
#include "BLEDevice.h"
#include "BLEServer.h"
#include "BLEUtils.h"
#include "BLE2902.h"
#include "String.h"

BLECharacteristic *pCharacteristic;
bool deviceConnected = false;
uint8_t txValue = 0;
long lastMsg = 0;
String rxload = "Test\n";

#define SERVICE_UUID "95b729c9-8e25-47b8-8967-8c9c666234c8"
#define CHARACTERISTIC_UUID_RX "b6ed4753-0338-48ae-88eb-a044fc97e718"
#define CHARACTERISTIC_UUID_TX "9440e3ce-f4d0-49b2-ad86-ebdbc821e233"
#define MOTOR_A1 16
#define MOTOR_A2 17
#define MOTOR_B1 25
#define MOTOR_B2 26

void stopCar();
void leftUp();    // 左轮前进
void rightUp();   // 右轮前进
void leftDown();  // 左轮后退
void rightDown(); // 右轮后退
class MyServerCallbacks : public BLEServerCallbacks
{
  void onConnect(BLEServer *pServer)
  {
    deviceConnected = true;
  };
  void onDisconnect(BLEServer *pServer)
  {
    deviceConnected = false;
  }
};

class MyCallbacks : public BLECharacteristicCallbacks
{
  void onWrite(BLECharacteristic *pCharacteristic)
  {
    std::string rxValue = pCharacteristic->getValue();
    bool state = false;
    if (rxValue.length() > 0)
    {
      // 和之前的值一样就保持原样
      if (strcmp(rxload.c_str(), rxValue.c_str()) == 0)
      {
        state = true;
      }
      rxload = "";
      for (int i = 0; i < rxValue.length(); i++)
      {
        rxload += (char)rxValue[i];
      }
    }
    Serial.println(rxload);
    if (state)
    {
      return;
    }
    if (strcmp(rxValue.c_str(), "up") == 0)
    {
      stopCar();
      delay(50);
      leftUp();
      rightUp();
    }
    else if (strcmp(rxValue.c_str(), "down") == 0)
    {
      stopCar();
      delay(50);
      leftDown();
      rightDown();
    }
    else if (strcmp(rxValue.c_str(), "left") == 0)
    {
      stopCar();
      delay(50);
      leftDown();
      rightUp();
      rxload = "";
      delay(80);
      stopCar();
    }
    else if (strcmp(rxValue.c_str(), "right") == 0)
    {
      stopCar();
      delay(50);
      leftUp();
      rightDown();
      rxload = "";
      delay(80);
      stopCar();
    }
    else if (strcmp(rxValue.c_str(), "start") == 0)
    {
    }
    else if (strcmp(rxValue.c_str(), "stop") == 0)
    {
      stopCar();
      delay(50);
    }
  }
};

void setupBLE(String BLEName)
{
  const char *ble_name = BLEName.c_str();
  BLEDevice::init(ble_name);
  Serial.println(BLEDevice::getAddress().toString().c_str());
  BLEServer *pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyServerCallbacks());
  BLEService *pService = pServer->createService(SERVICE_UUID);
  pCharacteristic = pService->createCharacteristic(CHARACTERISTIC_UUID_TX, BLECharacteristic::PROPERTY_NOTIFY);
  pCharacteristic->addDescriptor(new BLE2902());
  BLECharacteristic *pCharacteristic = pService->createCharacteristic(CHARACTERISTIC_UUID_RX, BLECharacteristic::PROPERTY_WRITE);
  pCharacteristic->setCallbacks(new MyCallbacks());
  pService->start();
  pServer->getAdvertising()->start();
  Serial.println("Waiting a client connection to notify...");
}
void stopCar()
{
  digitalWrite(MOTOR_A1, LOW);
  digitalWrite(MOTOR_A2, LOW);
  digitalWrite(MOTOR_B1, LOW);
  digitalWrite(MOTOR_B2, LOW);
}
void leftUp() // 左轮前进
{
  digitalWrite(MOTOR_A1, HIGH);
  digitalWrite(MOTOR_A2, LOW);
}
void rightUp() // 右轮前进
{
  digitalWrite(MOTOR_B1, HIGH);
  digitalWrite(MOTOR_B2, LOW);
}
void leftDown() // 左轮后退
{
  digitalWrite(MOTOR_A1, LOW);
  digitalWrite(MOTOR_A2, HIGH);
}
void rightDown() // 右轮后退
{
  digitalWrite(MOTOR_B1, LOW);
  digitalWrite(MOTOR_B2, HIGH);
}
void setup()
{
  Serial.begin(115200);
  pinMode(MOTOR_A1, OUTPUT);
  pinMode(MOTOR_A2, OUTPUT);
  pinMode(MOTOR_B1, OUTPUT);
  pinMode(MOTOR_B2, OUTPUT);
  stopCar();
  setupBLE("zhanxg_car");
}

void loop()
{
  // long now = millis();
  // if (now - lastMsg > 100) {
  //   if (deviceConnected&&rxload.length()>0) {
  //       Serial.println(rxload);
  //       rxload="";
  //   }
  //   lastMsg = now;
  // }
}
